#include <Servo.h>
#include <math.h>
#define PI 3.14159265
Servo S1;
Servo S2;
Servo S3;
int S1max=180,S1min=23;//maksymalny i minimalny kąt 1 serva
int S2max=180,S2min=0;//maksymalny i minimalny kąt 2 serva 
int a=179,b=145,c=53,e=90;//wymiary w mm
char mystr[20];
char dlug[3];
char wys[3];
char kat[3];

int dlugosc=-1, wysokosc=-1, kacik=-1;
bool q=false, w=false, e0=false,q1=false, w1=false, e1=false, q2=false, w2=false, e2=false, z=false, v=false, d=false;
int f=0;

void check(float,float,float);//funkcja liczy kąty jakie potrzebujem dla serva1 i serva2


void setup() {
  Serial.begin(9600);
  pinMode(A0,INPUT_PULLUP);
  S1.attach(9);//tuuuuuu
  S2.attach(10);//tuuuuuu
  S3.attach(11);
  S2.write(90);
  S1.write(90);
  delay(10);
  pinMode(12,OUTPUT);
  digitalWrite(13,LOW);
}
 
void loop() {
  if(z==false && v==false && Serial.available())
{ 
  Serial.readBytes(dlug,3);
if(dlug[0]>=48 && dlug[1]<48 && dlug[2]<48 && q==false)
{

dlugosc=((int)dlug[0]-48);
q=true;
}
if(dlug[0]>=48 && dlug[1]>=48 && dlug[2]<48 && w==false)
{

dlugosc=((int)dlug[0]-48)*10 +((int)dlug[1]-48);
w=true;
}
if(dlug[0]>=48 && dlug[1]>=48 && dlug[2]>=48 && e0==false)
{
dlugosc=((int)dlug[0]-48)*100 +((int)dlug[1]-48)*10+(int)dlug[2]-48;
e0=true;
}
z=true;
v=true;
}
if(v==true && d==false && Serial.available())
{
  Serial.readBytes(wys,3);
if(wys[0]>=48 && wys[1]<48 && wys[2]<48 && q1==false)
{

wysokosc=((int)wys[0]-48);
q1=true;
}
if(wys[0]>=48 && wys[1]>=48 && wys[2]<48 && w1==false)
{

wysokosc=((int)wys[0]-48)*10 +((int)wys[1]-48);
w1=true;
}
if(wys[0]>=48 && wys[1]>=48 && wys[2]>=48 && e1==false)
{
wysokosc=((int)wys[0]-48)*100 +((int)wys[1]-48)*10+(int)wys[2]-48;
e1=true;
}
v=false;
d=true;

}
if(d==true && z==true && Serial.available())
{
  Serial.readBytes(kat,3);
if(kat[0]>=48 && kat[1]<48 && kat[2]<48 && q2==false)
{

kacik=((int)kat[0]-48);
q2=true;
}
if(kat[0]>=48 && kat[1]>=48 && kat[2]<48 && w2==false)
{

kacik=((int)kat[0]-48)*10 +((int)kat[1]-48);

w2=true;
}
if(kat[0]>=48 && kat[1]>=48 && kat[2]>=48 && e2==false)
{
kacik=((int)kat[0]-48)*100 +((int)kat[1]-48)*10+(int)kat[2]-48;
e2=true;


}
d=false;
z=true;
}
if(dlugosc>=0 && wysokosc>=0)
{
  if(digitalRead(A0)==0)
  {
    digitalWrite(12,HIGH);
    

    int nom=S3.read();
    float katServ[2];
      check(dlugosc, wysokosc, katServ);
	if(nom<80){
     for(nom;nom<=80;nom++)
        {
        S3.write(nom);
        delay(50);
        } //koniec fora 
        }
    
   	idzie(katServ[0],katServ[1],50);
	delay(1000);
	
    if(nom>57){
     for(nom;nom>=57;nom--)
        {
        S3.write(nom);
        delay(50);
        } //koniec fora 
        }
	
	idzie(100,90,50);
  
    delay(1000);
    digitalWrite(12,LOW);
    }//przycisk
}
   
 }
void idzie(float pierwszy, float drugi, int szybkosc)
{
int i = S2.read();
int j = S1.read();
if(i<drugi){
     for(i;i<=drugi;i++)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
    if(i>drugi){
     for(i;i>=drugi;i--)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
    if(j<pierwszy){
    for(j;j<=pierwszy;j++)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }
    if(j>pierwszy){
    for(j;j>=pierwszy;j--)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }


}//koniec szybkosc

void check(float odlx,float wysy,float katServ[])
{
    int i,j;
    float h,X,D,y1,y2,kat1,kat2,odl,roznica,alfa,beta,odwrot;
    roznica=100;
    

    for (i=S1min;i<=S1max;i++)
    {
        for (j=S2min; j<=S2max;j++)
        {   
          odwrot=180-j;
            alfa=abs(i-90);
            if(i>=90)
            {   if((j+alfa)<180)
                beta=abs((odwrot-90+alfa));
                else
                beta=odwrot-180+alfa;
                
            }
            else
            {   
                if(odwrot<=alfa)
                beta=abs(alfa-odwrot);
                else
                beta=abs((odwrot-90-alfa));
                
            }
          
       
      kat1= alfa/180*PI;//kąt w radianach
            kat2= beta/180*PI;
          
            if(((i>=90) && ((odwrot+alfa)>=180)) || ((i<90) && (odwrot<=alfa)))
              {
                D=cos(kat2)*c;
                y2=sin(kat2)*c;
                X=cos(kat1)*b;
                y1=sin(kat1)*b;
              }
            else
              {
                X=cos(kat1)*b;
                D=sin(kat2)*c;
                y1=sin(kat1)*b;
                y2=cos(kat2)*c;
              }

            if((i>=90) && ((odwrot+alfa)>=180))
            h=a+y1+y2-e;
            else if((i>=90) && ((odwrot+alfa)<180))
            h=a+y1-y2-e;
            else if((i<90) && (odwrot<=alfa))
            h=a-y1+y2-e;
            else if((i<90) && (odwrot>alfa))
            h=a-y1-y2-e;

            if(((odwrot>=(90-alfa)) && (i>=90)) || ((i<90) &&  (odwrot>=(90+alfa))))
            odl=X+D;
            else
            odl=X-D;

            
            if ((roznica>=(abs(odlx-odl)+abs(wysy-h))) && (wysy<=h))
            { Serial.print(".  if.  ");
                roznica=(abs(odlx-odl)+abs(wysy-h));//wazne
                katServ[0]=i+10; 
                katServ[1]=j; 
               
            }

        }
    }

}//koniec check